﻿

using DualPlaneFR4Paster.Common.Services;
using HalconDotNet;
using Prism.Commands;
using Prism.Ioc;
using Prism.Services.Dialogs;
using System;
using System.Collections.ObjectModel;
using System.Diagnostics;
using ViewROI;
using System.Threading.Tasks;
using System.Threading;
using System.Windows.Forms;
using DualPlaneFR4Paster.Models;
using System.Linq;
using NLog;
using System.Collections.Immutable;
using System.Reflection.Metadata;
using DualPlaneFR4Paster.Extensions;
using Microsoft.EntityFrameworkCore.Metadata.Internal;

namespace DualPlaneFR4Paster.ViewModels.Dialogs.Camera
{
    public class CameraFilmCalibDialogViewModel : DialogViewModel
    {
        #region 变量
        private static Logger logger = LogManager.GetCurrentClassLogger();
        private int floor = 1;
        private string partNum = string.Empty;
        private readonly IContainerProvider _containerProvider;
        private ICameraService cam1;
        CancellationTokenSource source;
        private IGTSCardService Top_A4;
        private IGTSCardService Top_A8;
        private IGTSCardService Bottom_A8;
        double xjogspeed = 0;
        double yjogspeed = 0;
        double zjogspeed = 0;
        double rjogspeed = 0;
        double z1safe = 0;
        double z2safe = 0;
        double z3safe = 0;
        double z4safe = 0;
        #endregion
        #region 属性绑定
        #region halcon
        #region 1#
        private HImage cameraIamge0;
        public HImage CameraIamge0
        {
            get { return cameraIamge0; }
            set { SetProperty(ref cameraIamge0, value); }
        }
        private bool cameraRepaint0;
        public bool CameraRepaint0
        {
            get { return cameraRepaint0; }
            set { SetProperty(ref cameraRepaint0, value); }
        }
        private ObservableCollection<ROI> cameraROIList0 = new ObservableCollection<ROI>();
        public ObservableCollection<ROI> CameraROIList0
        {
            get { return cameraROIList0; }
            set { SetProperty(ref cameraROIList0, value); }
        }
        private HObject cameraAppendHObject0;
        public HObject CameraAppendHObject0
        {
            get { return cameraAppendHObject0; }
            set { SetProperty(ref cameraAppendHObject0, value); }
        }
        private HMsgEntry cameraAppendHMessage0;
        public HMsgEntry CameraAppendHMessage0
        {
            get { return cameraAppendHMessage0; }
            set { SetProperty(ref cameraAppendHMessage0, value); }
        }
        private Tuple<string, object> cameraGCStyle0;
        public Tuple<string, object> CameraGCStyle0
        {
            get { return cameraGCStyle0; }
            set { SetProperty(ref cameraGCStyle0, value); }
        }
        #endregion
        #endregion
        private int suckerSelectedIndex;
        public int SuckerSelectedIndex
        {
            get { return suckerSelectedIndex; }
            set { SetProperty(ref suckerSelectedIndex, value); }
        }
        private ObservableCollection<AxisPointDisp> pointList1;
        public ObservableCollection<AxisPointDisp> PointList1
        {
            get { return pointList1; }
            set { SetProperty(ref pointList1, value); }
        }
        private bool isBusy;
        public bool IsBusy
        {
            get { return isBusy; }
            set { SetProperty(ref isBusy, value); }
        }
        private ObservableCollection<ParamDisp> paramList1;
        public ObservableCollection<ParamDisp> ParamList1
        {
            get { return paramList1; }
            set { SetProperty(ref paramList1, value); }
        }
        #endregion
        #region 方法绑定
        private DelegateCommand<object> cameraOperateCommand;
        public DelegateCommand<object> CameraOperateCommand =>
            cameraOperateCommand ?? (cameraOperateCommand = new DelegateCommand<object>(ExecuteCameraOperateCommand));
        private DelegateCommand<object> liveGrabCommand;
        public DelegateCommand<object> LiveGrabCommand =>
            liveGrabCommand ?? (liveGrabCommand = new DelegateCommand<object>(ExecuteLiveGrabCommand));
        private DelegateCommand<object> startCalibCommand;
        public DelegateCommand<object> StartCalibCommand =>
            startCalibCommand ?? (startCalibCommand = new DelegateCommand<object>(ExecuteStartCalibCommand));
        private DelegateCommand createShapeModelCommand;
        public DelegateCommand CreateShapeModelCommand =>
            createShapeModelCommand ?? (createShapeModelCommand = new DelegateCommand(ExecuteCreateShapeModelCommand));
        private DelegateCommand findShapeModelCommand;
        public DelegateCommand FindShapeModelCommand =>
            findShapeModelCommand ?? (findShapeModelCommand = new DelegateCommand(ExecuteFindShapeModelCommand));
        private DelegateCommand<AxisPointDisp> pointList1JumpPosCommand;
        public DelegateCommand<AxisPointDisp> PointList1JumpPosCommand =>
            pointList1JumpPosCommand ?? (pointList1JumpPosCommand = new DelegateCommand<AxisPointDisp>(ExecutePointList1JumpPosCommand));
        private DelegateCommand<object> suckerIndexSelectCommand;
        public DelegateCommand<object> SuckerIndexSelectCommand =>
            suckerIndexSelectCommand ?? (suckerIndexSelectCommand = new DelegateCommand<object>(ExecuteSuckerIndexSelectCommand));
        private DelegateCommand<ParamDisp> paramList1RowEditEndingCommand;
        public DelegateCommand<ParamDisp> ParamList1RowEditEndingCommand =>
            paramList1RowEditEndingCommand ?? (paramList1RowEditEndingCommand = new DelegateCommand<ParamDisp>(ExecuteParamList1RowEditEndingCommand));

        void ExecuteParamList1RowEditEndingCommand(ParamDisp param)
        {
            if (param != null)
            {
                using (var db = new DTContext(partNum))
                {
                    var _param = db.MParams.FirstOrDefault(p => p.Id == param.Id);
                    if (_param != null)
                    {
                        _param.Value = param.Value.ToString();
                        db.SaveChanges();
                    }
                }
            }
        }
        void ExecuteSuckerIndexSelectCommand(object obj)
        {
            if (obj != null)
            {
                SuckerSelectedIndex = int.Parse(obj.ToString());
                Debug.WriteLine(obj.ToString());
            }
        }
        async void ExecutePointList1JumpPosCommand(AxisPointDisp point)
        {
            if (point != null)
            {
                var result = _dialogHostService.MessageBox(MessageType.Confirm, "信息", $"确认Jump运动到点\"{point.Alias}\"吗？");
                if (result == ButtonResult.Yes)
                {

                    IsBusy = true;
                    source = new CancellationTokenSource();
                    CancellationToken token = source.Token;
                    await Task.Run(() => JumpXYZRAction(token, point), token).ContinueWith(t => { IsBusy = false; });
                }

            }
        }
        void ExecuteFindShapeModelCommand()
        {
            try
            {
                string filepath = $"Camera\\{(floor == 1 ? "Top" : "Bottom")}\\Calib\\Down";
                HTuple ModelID;
                HOperatorSet.ReadShapeModel(System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath, $"CalibShapeModelID{SuckerSelectedIndex}.shm"), out ModelID);
                HTuple Row, Column, Angle, Score;
                HOperatorSet.FindShapeModel(CameraIamge0, ModelID, new HTuple(-180).TupleRad(), new HTuple(360).TupleRad(), 0.5, 1, 0.5, "least_squares", new HTuple(5).TupleConcat(1), 0.75, out Row, out Column, out Angle, out Score);
                HOperatorSet.ClearShapeModel(ModelID);
                HTuple HomMat2D;
                HOperatorSet.HomMat2dIdentity(out HomMat2D);
                HOperatorSet.HomMat2dRotate(HomMat2D, Angle, 0, 0, out HomMat2D);
                HOperatorSet.HomMat2dTranslate(HomMat2D, Row, Column, out HomMat2D);
                HTuple hv_DxfStatus; HObject ModelContours;
                HOperatorSet.ReadContourXldDxf(out ModelContours, System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath, $"CalibShapeModelContours{SuckerSelectedIndex}.dxf"), new HTuple(), new HTuple(), out hv_DxfStatus);
                HObject TransContours;
                HOperatorSet.AffineTransContourXld(ModelContours, out TransContours, HomMat2D);
                CameraAppendHObject0 = null;
                CameraGCStyle0 = new Tuple<string, object>("Color", "green");
                CameraGCStyle0 = new Tuple<string, object>("DrawMode", "margin");
                CameraAppendHObject0 = TransContours;
                CameraAppendHMessage0 = null;
                CameraAppendHMessage0 = new HMsgEntry($"坐标:{Row.D:F1},{Column.D:F1},{Angle.TupleDeg().D:F1}° 分数:{Score.D:F1}", 100, 100);
            }
            catch (Exception ex)
            {
                _dialogHostService.MessageBox(MessageType.Message, "信息", ex.Message);
            }
        }
        void ExecuteCreateShapeModelCommand()
        {
            var result = _dialogHostService.MessageBox(MessageType.Confirm, "信息", $"确定重新画\"吸头{SuckerSelectedIndex + 1}\"的标定模板吗？");
            if (result == ButtonResult.Yes)
            {
                string filepath = $"Camera\\{(floor == 1 ? "Top" : "Bottom")}\\Calib\\Down";
                try
                {
                    ROI roi = Global.CameraImageViewer.DrawROI(ROI.ROI_TYPE_REGION);
                    HRegion ModelRegion = roi.getRegion();
                    HObject TemplateImage;
                    HOperatorSet.ReduceDomain(CameraIamge0, ModelRegion, out TemplateImage);
                    HTuple ModelID;
                    HOperatorSet.CreateShapeModel(TemplateImage, 3, new HTuple(-180).TupleRad(), new HTuple(360).TupleRad(), new HTuple(1).TupleRad(), new HTuple("none").TupleConcat("no_pregeneration"), "use_polarity", new HTuple(30).TupleConcat(30).TupleConcat(0), 10, out ModelID);
                    HObject ModelContours;
                    HOperatorSet.GetShapeModelContours(out ModelContours, ModelID, 1);
                    HTuple ModelRegionArea, RefRow, RefColumn;
                    HOperatorSet.AreaCenter(ModelRegion, out ModelRegionArea, out RefRow, out RefColumn);
                    HTuple HomMat2D;
                    HOperatorSet.VectorAngleToRigid(0, 0, 0, RefRow, RefColumn, 0, out HomMat2D);
                    HObject TransContours;
                    HOperatorSet.AffineTransContourXld(ModelContours, out TransContours, HomMat2D);
                    CameraAppendHObject0 = null;
                    CameraGCStyle0 = new Tuple<string, object>("Color", "green");
                    CameraGCStyle0 = new Tuple<string, object>("DrawMode", "margin");
                    CameraAppendHObject0 = TransContours;
                    CameraAppendHMessage0 = null;
                    CameraAppendHMessage0 = new HMsgEntry($"坐标:{RefRow.D:F1},{RefColumn.D:F1}", 100, 100);
                    HOperatorSet.WriteShapeModel(ModelID, System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath, $"CalibShapeModelID{SuckerSelectedIndex}.shm"));
                    HOperatorSet.ClearShapeModel(ModelID);
                    HOperatorSet.WriteContourXldDxf(ModelContours, System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath, $"CalibShapeModelContours{SuckerSelectedIndex}.dxf"));

                }
                catch (Exception ex)
                {
                    _dialogHostService.MessageBox(MessageType.Message, "信息", ex.Message);
                }
            }
               
        }
        void ExecuteStartCalibCommand(object obj)
        {
            var result = _dialogHostService.MessageBox(MessageType.Confirm, "信息", "确定重新标定吗？");
            if (result == ButtonResult.Yes)
            {
                if ((bool)obj)
                {
                    switch (SuckerSelectedIndex)
                    {
                        case 0:
                            {
                                IsBusy = true;
                                source = new CancellationTokenSource();
                                CancellationToken token = source.Token;
                                var _point = PointList1.FirstOrDefault(p => p.Name == "BottomCameraCalib_Sucker1");
                                var _dist = ParamList1.FirstOrDefault(p => p.Name == "DownCameraCalibMoveDistance");
                                var _angle = ParamList1.FirstOrDefault(p => p.Name == "DownCameraCalibMoveAngle");
                                if (_point != null && _dist != null && _angle != null)
                                {
                                    double dist = double.Parse(_dist.Value.ToString());
                                    double angle = double.Parse(_angle.Value.ToString());
                                    Task.Run(() => CalibMove12Point(token, _point, dist, angle), token).ContinueWith(t => {
                                        CalibCalc(_point, dist, angle);
                                        IsBusy = false;
                                    });
                                }

                            }
                            break;
                        case 1:
                        case 2:
                        case 3:
                            {
                                IsBusy = true;
                                source = new CancellationTokenSource();
                                CancellationToken token = source.Token;
                                var _point = PointList1.FirstOrDefault(p => p.Name == $"BottomCameraCalib_Sucker{SuckerSelectedIndex + 1}");
                                var _angle = ParamList1.FirstOrDefault(p => p.Name == "DownCameraCalibMoveAngle");
                                if (_point != null && _angle != null)
                                {
                                    double angle = double.Parse(_angle.Value.ToString());
                                    Task.Run(() => CalibMoveCenter(token, _point, angle, SuckerSelectedIndex + 1), token).ContinueWith(t =>
                                    {
                                        CalibCalcRelation(_point, SuckerSelectedIndex + 1);
                                        IsBusy = false;
                                    });
                                }
                            }
                            break;
                        default:
                            break;
                    }
                }
                else
                {
                    switch (SuckerSelectedIndex)
                    {
                        case 0:
                            {
                                var _point = PointList1.FirstOrDefault(p => p.Name == "BottomCameraCalib_Sucker1");
                                var _dist = ParamList1.FirstOrDefault(p => p.Name == "DownCameraCalibMoveDistance");
                                var _angle = ParamList1.FirstOrDefault(p => p.Name == "DownCameraCalibMoveAngle");
                                if (_point != null && _dist != null && _angle != null)
                                {
                                    double dist = double.Parse(_dist.Value.ToString());
                                    double angle = double.Parse(_angle.Value.ToString());
                                    CalibCalc(_point, dist, angle);
                                }
                            }
                            break;
                        case 1:
                        case 2:
                        case 3:
                            {
                                var _point = PointList1.FirstOrDefault(p => p.Name == $"BottomCameraCalib_Sucker{SuckerSelectedIndex + 1}");
                                if (_point != null)
                                {
                                    CalibCalcRelation(_point, SuckerSelectedIndex + 1);
                                }
                            }
                            break;
                        default:
                            break;
                    }
                }
            }
              
        }
        void ExecuteLiveGrabCommand(object isLiveGrab)
        {        
            if ((bool)isLiveGrab)
            {
                source = new CancellationTokenSource();
                CancellationToken token = source.Token;
                Task.Run(() => ContinueGrab(token), token);
            }
            else
            {
                if (source != null)
                {
                    source.Cancel();
                }
            }
        }
        void ExecuteCameraOperateCommand(object obj)
        {
            switch (obj.ToString())
            {
                case "拍照":
                    {
                        cam1.ImageQueue.Clear();
                        cam1.SetTriggerMode(0);
                        cam1.StartGrabe();
                        Task.Run(() => {
                            int mIndex = 0;
                            while (mIndex < 1)
                            {
                                HObject? hObj;
                                if (cam1.ImageQueue.TryPeek(out hObj))
                                {
                                    cam1.ImageQueue.TryDequeue(out hObj);
                                    CameraIamge0 = new HImage(hObj);
                                    mIndex++;
                                }
                            }                            
                        }).ContinueWith(t => { cam1.StopGrabe(); });
                    }
                    break;
                case "打开":
                    {
                        OpenFileDialog openFileDialog = new OpenFileDialog();
                        openFileDialog.Filter = "Image Files|*.png;*.bmp;*.jpg;*.tif";
                        if (openFileDialog.ShowDialog() == System.Windows.Forms.DialogResult.OK)
                        {
                            HObject image;
                            HOperatorSet.ReadImage(out image, openFileDialog.FileName);
                            CameraIamge0 = new HImage(image);
                        }
                    }
                    break;
                case "增强":
                    {
                        HObject imageScaled;
                        HTuple mult = 255.0 / (255 - 70);
                        HTuple add = -1 * mult * 70;
                        HOperatorSet.ScaleImage(CameraIamge0, out imageScaled, mult, add);
                        CameraIamge0 = new HImage(imageScaled);
                    }
                    break;
                default:
                    break;
            }
        }
        #endregion
        #region 导航
        public override void OnDialogOpened(IDialogParameters parameters)
        {
            floor = parameters.GetValue<int>("Floor");
            partNum = parameters.GetValue<string>("PartNum");
            Title = $"{(floor == 1 ? "上层" : "下层")}-{partNum}-拍膜相机 标定";
            switch (floor)
            {
                case 1:
                    cam1 = _containerProvider.Resolve<ICameraService>("Floor1Down");
                    break;
                case 2:
                    cam1 = _containerProvider.Resolve<ICameraService>("Floor2Down");
                    break;
                default:
                    break;
            }
            using (var db = new DTContext(partNum))
            {
                var points = db.XYZRPoints.Where(p => (p.Name == "BottomCameraCalib_Sucker1" || p.Name == "BottomCameraCalib_Sucker2" || p.Name == "BottomCameraCalib_Sucker3" || p.Name == "BottomCameraCalib_Sucker4") && p.Floor == floor);
                if (points.Any())
                {
                    var posList = points.ToList();
                    foreach (var item in posList)
                    {
                        PointList1.Add(
                            new AxisPointDisp
                            {
                                Id = item.Id,
                                Name = item.Name,
                                Alias = item.Alias,
                                X = item.X,
                                Y = item.Y,
                                Z = item.Z,
                                R = item.R,
                                ZIndex = item.ZIndex,
                                RIndex = item.RIndex
                            }
                            );
                    }
                }
                var v1 = db.MParams.FirstOrDefault(p => p.Name == "XJogSpeed" && p.Floor == floor);
                if (v1 != null)
                {
                    xjogspeed = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "YJogSpeed" && p.Floor == floor);
                if (v1 != null)
                {
                    yjogspeed = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "ZJogSpeed" && p.Floor == floor);
                if (v1 != null)
                {
                    zjogspeed = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "RJogSpeed" && p.Floor == floor);
                if (v1 != null)
                {
                    rjogspeed = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "Z1SafePos" && p.Floor == floor);
                if (v1 != null)
                {
                    z1safe = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "Z2SafePos" && p.Floor == floor);
                if (v1 != null)
                {
                    z2safe = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "Z3SafePos" && p.Floor == floor);
                if (v1 != null)
                {
                    z3safe = double.Parse(v1.Value);
                }
                v1 = db.MParams.FirstOrDefault(p => p.Name == "Z4SafePos" && p.Floor == floor);
                if (v1 != null)
                {
                    z4safe = double.Parse(v1.Value);
                }
                var dists = db.MParams.Where(p => (p.Name == "DownCameraCalibMoveDistance" || p.Name == "DownCameraCalibMoveAngle") && p.Floor == floor);
                if (dists.Any())
                {
                    foreach (var item in dists.ToList())
                    {
                        ParamList1.Add(new ParamDisp { 
                            Id = item.Id,
                            Name = item.Name,
                            Value = item.Value,
                            Alias = item.Alias,
                            Unit = item.Unit
                        });
                    }
                }
            }
            switch (floor)
            {
                case 1:
                    {
                        GTSCard.ServoOn(Top_A8.A1);//X
                        GTSCard.ServoOn(Top_A8.A2);//Y
                        GTSCard.ServoOn(Top_A8.A4);//Z1
                        GTSCard.ServoOn(Top_A8.A5);//Z2
                        GTSCard.ServoOn(Top_A8.A6);//Z3
                        GTSCard.ServoOn(Top_A8.A7);//Z4
                        GTSCard.ServoOn(Top_A4.A1);//R1
                        GTSCard.ServoOn(Top_A4.A2);//R2

                        GTSCard.AxisPosSet(Top_A8.A1, GTSCard.GetEnc(Top_A8.A1));
                        GTSCard.AxisPosSet(Top_A8.A2, GTSCard.GetEnc(Top_A8.A2));
                        GTSCard.AxisPosSet(Top_A8.A4, GTSCard.GetEnc(Top_A8.A4));
                        GTSCard.AxisPosSet(Top_A8.A5, GTSCard.GetEnc(Top_A8.A5));
                        GTSCard.AxisPosSet(Top_A8.A6, GTSCard.GetEnc(Top_A8.A6));
                        GTSCard.AxisPosSet(Top_A8.A7, GTSCard.GetEnc(Top_A8.A7));
                        GTSCard.AxisPosSet(Top_A4.A1, GTSCard.GetEnc(Top_A4.A1));
                        GTSCard.AxisPosSet(Top_A4.A2, GTSCard.GetEnc(Top_A4.A2));
                    }
                    break;
                case 2:
                    {
                        GTSCard.ServoOn(Bottom_A8.A1);//X
                        GTSCard.ServoOn(Bottom_A8.A2);//Y
                        GTSCard.ServoOn(Bottom_A8.A4);//Z1
                        GTSCard.ServoOn(Bottom_A8.A5);//Z2
                        GTSCard.ServoOn(Bottom_A8.A6);//Z3
                        GTSCard.ServoOn(Bottom_A8.A7);//Z4
                        GTSCard.ServoOn(Top_A4.A3);//R1
                        GTSCard.ServoOn(Top_A4.A4);//R2

                        GTSCard.AxisPosSet(Bottom_A8.A1, GTSCard.GetEnc(Bottom_A8.A1));
                        GTSCard.AxisPosSet(Bottom_A8.A2, GTSCard.GetEnc(Bottom_A8.A2));
                        GTSCard.AxisPosSet(Bottom_A8.A4, GTSCard.GetEnc(Bottom_A8.A4));
                        GTSCard.AxisPosSet(Bottom_A8.A5, GTSCard.GetEnc(Bottom_A8.A5));
                        GTSCard.AxisPosSet(Bottom_A8.A6, GTSCard.GetEnc(Bottom_A8.A6));
                        GTSCard.AxisPosSet(Bottom_A8.A7, GTSCard.GetEnc(Bottom_A8.A7));
                        GTSCard.AxisPosSet(Top_A4.A3, GTSCard.GetEnc(Top_A4.A3));
                        GTSCard.AxisPosSet(Top_A4.A4, GTSCard.GetEnc(Top_A4.A4));
                    }
                    break;
                default:
                    break;
            }
        }
        public override void OnDialogClosed()
        {
            cam1.StopGrabe();
            if (source != null)
            {
                source.Cancel();
            }
        }
        #endregion
        public CameraFilmCalibDialogViewModel(IContainerProvider containerProvider) : base(containerProvider)
        {
            _containerProvider = containerProvider;
            SuckerSelectedIndex = 0;
            PointList1 = new ObservableCollection<AxisPointDisp>();
            ParamList1 = new ObservableCollection<ParamDisp>();
            IsBusy = false;
            Top_A4 = containerProvider.Resolve<IGTSCardService>("Top_A4");
            Top_A8 = containerProvider.Resolve<IGTSCardService>("Top_A8");
            Bottom_A8 = containerProvider.Resolve<IGTSCardService>("Bottom_A8");
        }
        #region 功能函数
        private void ContinueGrab(CancellationToken token)
        {
            cam1.ImageQueue.Clear();
            cam1.SetTriggerMode(0);
            cam1.StartGrabe();
            while (true)
            {
                if (token.IsCancellationRequested)
                {
                    cam1.StopGrabe();
                    return;
                }
                HObject? hObj;
                if (cam1.ImageQueue.TryPeek(out hObj))
                {
                    cam1.ImageQueue.TryDequeue(out hObj);
                    CameraIamge0 = new HImage(hObj);           
                }
                System.Threading.Thread.Sleep(1);
            }
        }
        private void JumpXYZRAction(CancellationToken token, AxisPointDisp point)
        {
            try
            {
                var axisX = floor == 1 ? Top_A8.A1 : Bottom_A8.A1;
                var axisY = floor == 1 ? Top_A8.A2 : Bottom_A8.A2;
                var axisZ1 = floor == 1 ? Top_A8.A4 : Bottom_A8.A4;
                var axisZ2 = floor == 1 ? Top_A8.A5 : Bottom_A8.A5;
                var axisZ3 = floor == 1 ? Top_A8.A6 : Bottom_A8.A6;
                var axisZ4 = floor == 1 ? Top_A8.A7 : Bottom_A8.A7;
                AxisParm axisZ, axisR;
                switch (point.ZIndex)
                {
                    case 0:
                        axisZ = floor == 1 ? Top_A8.A4 : Bottom_A8.A4;
                        break;
                    case 1:
                        axisZ = floor == 1 ? Top_A8.A5 : Bottom_A8.A5;
                        break;
                    case 2:
                        axisZ = floor == 1 ? Top_A8.A6 : Bottom_A8.A6;
                        break;
                    case 3:
                    default:
                        axisZ = floor == 1 ? Top_A8.A7 : Bottom_A8.A7;
                        break;
                }
                switch (point.RIndex)
                {
                    case 0:
                        axisR = floor == 1 ? Top_A4.A1 : Top_A4.A3;
                        break;
                    case 1:
                    default:
                        axisR = floor == 1 ? Top_A4.A2 : Top_A4.A4;
                        break;
                }
                int stepnum = 0;
                while (true)
                {
                    if (token.IsCancellationRequested)
                    {
                        return;
                    }
                    switch (stepnum)
                    {
                        case 0://Z运动到安全位
                            {
                                GTSCard.AxisPosMove(ref axisZ1, z1safe, zjogspeed);
                                GTSCard.AxisPosMove(ref axisZ2, z2safe, zjogspeed);
                                GTSCard.AxisPosMove(ref axisZ3, z3safe, zjogspeed);
                                GTSCard.AxisPosMove(ref axisZ4, z4safe, zjogspeed);
                                stepnum = 1;
                            }
                            break;
                        case 1:
                            if (GTSCard.AxisPosMoveCheckDone(axisZ1) && GTSCard.AxisPosMoveCheckDone(axisZ2) && GTSCard.AxisPosMoveCheckDone(axisZ3) && GTSCard.AxisPosMoveCheckDone(axisZ4))
                            {
                                stepnum = 2;
                            }
                            break;
                        case 2:
                            {
                                GTSCard.AxisPosMove(ref axisX, point.X, xjogspeed);
                                GTSCard.AxisPosMove(ref axisY, point.Y, yjogspeed);
                                GTSCard.AxisPosMove(ref axisR, point.R, rjogspeed);
                                stepnum = 3;
                            }
                            break;
                        case 3:
                            if (GTSCard.AxisPosMoveCheckDone(axisX) && GTSCard.AxisPosMoveCheckDone(axisY) && GTSCard.AxisPosMoveCheckDone(axisR))
                            {
                                stepnum = 4;
                            }
                            break;
                        case 4:
                            {
                                GTSCard.AxisPosMove(ref axisZ, point.Z, zjogspeed);
                                stepnum = 5;
                            }
                            break;
                        case 5:
                            if (GTSCard.AxisPosMoveCheckDone(axisZ))
                            {
                                return;
                            }
                            break;
                        default:
                            break;
                    }
                    System.Threading.Thread.Sleep(100);
                }
            }
            catch (Exception ex)
            {
                logger.Error(ex);
                Debug.WriteLine(ex.Message);
            }

        }        
        private void CalibMove12Point(CancellationToken token, AxisPointDisp point, double dist, double angle)
        {
            try
            {
                string filepath = $"Camera\\{(floor == 1 ? "Top" : "Bottom")}\\Calib\\Down\\images";
                var axisX = floor == 1 ? Top_A8.A1 : Bottom_A8.A1;
                var axisY = floor == 1 ? Top_A8.A2 : Bottom_A8.A2;
                AxisParm axisZ, axisR;
                switch (point.ZIndex)
                {
                    case 0:
                        axisZ = floor == 1 ? Top_A8.A4 : Bottom_A8.A4;
                        break;
                    case 1:
                        axisZ = floor == 1 ? Top_A8.A5 : Bottom_A8.A5;
                        break;
                    case 2:
                        axisZ = floor == 1 ? Top_A8.A6 : Bottom_A8.A6;
                        break;
                    case 3:
                    default:
                        axisZ = floor == 1 ? Top_A8.A7 : Bottom_A8.A7;
                        break;
                }
                switch (point.RIndex)
                {
                    case 0:
                        axisR = floor == 1 ? Top_A4.A1 : Top_A4.A3;
                        break;
                    case 1:
                    default:
                        axisR = floor == 1 ? Top_A4.A2 : Top_A4.A4;
                        break;
                }
                double[][] diffArray = new double[9][]
                                        { new double[]{ 0,0,0},
                                     new double[]{ dist * 1, 0,0} ,
                                     new double[]{ dist * 1, dist * 1,0},
                                     new double[]{ 0, dist * 1,0},
                                     new double[]{ dist * -1, dist * 1,0},
                                     new double[]{ dist * -1,0,0},
                                     new double[]{ dist * -1, dist * -1,0},
                                     new double[]{ 0, dist * -1,0},
                                     new double[]{ dist * 1, dist * -1,0}};
                //跑9点
                for (int i = 0; i < 9; i++)
                {
                    GTSCard.AxisPosMove(ref axisX, point.X + diffArray[i][0], xjogspeed);
                    GTSCard.AxisPosMove(ref axisY, point.Y + diffArray[i][1], yjogspeed);
                    GTSCard.AxisPosMove(ref axisR, point.R, rjogspeed);
                    GTSCard.AxisPosMove(ref axisZ, point.Z, zjogspeed);
                    while (!GTSCard.AxisPosMoveCheckDone(axisX) || !GTSCard.AxisPosMoveCheckDone(axisY) || !GTSCard.AxisPosMoveCheckDone(axisZ) || !GTSCard.AxisPosMoveCheckDone(axisR))
                    {
                        System.Threading.Thread.Sleep(100);
                        if (token.IsCancellationRequested)
                        {
                            return;
                        }
                    }
                    System.Threading.Thread.Sleep(200);
                    cam1.ImageQueue.Clear();
                    cam1.SetTriggerMode(0);
                    cam1.StartGrabe();
                    int mIndex = 0;
                    while (mIndex < 1)
                    {
                        HObject? hObj;
                        if (cam1.ImageQueue.TryPeek(out hObj))
                        {
                            cam1.ImageQueue.TryDequeue(out hObj);
                            CameraIamge0 = new HImage(hObj);
                            mIndex++;
                        }
                    }
                    cam1.StopGrabe();
                    HOperatorSet.WriteImage(CameraIamge0, "jpeg", 0, System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath, $"CalibImage{i + 1}.jpg"));
                }
                //跑3点
                double[] diffArray1 = new double[3] { 0, angle * 1, angle * -1 };
                for (int i = 0; i < 3; i++)
                {
                    GTSCard.AxisPosMove(ref axisX, point.X, xjogspeed);
                    GTSCard.AxisPosMove(ref axisY, point.Y, yjogspeed);
                    GTSCard.AxisPosMove(ref axisR, point.R + diffArray1[i], rjogspeed);
                    while (!GTSCard.AxisPosMoveCheckDone(axisX) || !GTSCard.AxisPosMoveCheckDone(axisY) || !GTSCard.AxisPosMoveCheckDone(axisR))
                    {
                        System.Threading.Thread.Sleep(100);
                        if (token.IsCancellationRequested)
                        {
                            return;
                        }
                    }
                    System.Threading.Thread.Sleep(200);
                    cam1.ImageQueue.Clear();
                    cam1.SetTriggerMode(0);
                    cam1.StartGrabe();
                    int mIndex = 0;
                    while (mIndex < 1)
                    {
                        HObject? hObj;
                        if (cam1.ImageQueue.TryPeek(out hObj))
                        {
                            cam1.ImageQueue.TryDequeue(out hObj);
                            CameraIamge0 = new HImage(hObj);
                            mIndex++;
                        }
                    }
                    cam1.StopGrabe();
                    HOperatorSet.WriteImage(CameraIamge0, "jpeg", 0, System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath, $"CalibImage{i + 10}.jpg"));
                }
            }
            catch (Exception ex)
            {
                System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                {
                    _dialogHostService.MessageBox(MessageType.Message, "信息", ex.Message);
                }));
            }
        }
        private void CalibMoveCenter(CancellationToken token, AxisPointDisp point, double angle,int index)
        {
            try
            {
                string filepath = $"Camera\\{(floor == 1 ? "Top" : "Bottom")}\\Calib\\Down\\relation\\{index}";
                var axisX = floor == 1 ? Top_A8.A1 : Bottom_A8.A1;
                var axisY = floor == 1 ? Top_A8.A2 : Bottom_A8.A2;
                AxisParm axisZ, axisR;
                switch (point.ZIndex)
                {
                    case 0:
                        axisZ = floor == 1 ? Top_A8.A4 : Bottom_A8.A4;
                        break;
                    case 1:
                        axisZ = floor == 1 ? Top_A8.A5 : Bottom_A8.A5;
                        break;
                    case 2:
                        axisZ = floor == 1 ? Top_A8.A6 : Bottom_A8.A6;
                        break;
                    case 3:
                    default:
                        axisZ = floor == 1 ? Top_A8.A7 : Bottom_A8.A7;
                        break;
                }
                switch (point.RIndex)
                {
                    case 0:
                        axisR = floor == 1 ? Top_A4.A1 : Top_A4.A3;
                        break;
                    case 1:
                    default:
                        axisR = floor == 1 ? Top_A4.A2 : Top_A4.A4;
                        break;
                }               
                //跑3点
                double[] diffArray1 = new double[3] { 0, angle * 1, angle * -1 };
                for (int i = 0; i < 3; i++)
                {
                    GTSCard.AxisPosMove(ref axisX, point.X, xjogspeed);
                    GTSCard.AxisPosMove(ref axisY, point.Y, yjogspeed);
                    GTSCard.AxisPosMove(ref axisR, point.R + diffArray1[i], rjogspeed);
                    while (!GTSCard.AxisPosMoveCheckDone(axisX) || !GTSCard.AxisPosMoveCheckDone(axisY) || !GTSCard.AxisPosMoveCheckDone(axisR))
                    {
                        System.Threading.Thread.Sleep(100);
                        if (token.IsCancellationRequested)
                        {
                            return;
                        }
                    }
                    System.Threading.Thread.Sleep(200);
                    cam1.ImageQueue.Clear();
                    cam1.SetTriggerMode(0);
                    cam1.StartGrabe();
                    int mIndex = 0;
                    while (mIndex < 1)
                    {
                        HObject? hObj;
                        if (cam1.ImageQueue.TryPeek(out hObj))
                        {
                            cam1.ImageQueue.TryDequeue(out hObj);
                            CameraIamge0 = new HImage(hObj);
                            mIndex++;
                        }
                    }
                    cam1.StopGrabe();
                    HOperatorSet.WriteImage(CameraIamge0, "jpeg", 0, System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath, $"CalibImage{i + 10}.jpg"));
                }
            }
            catch (Exception ex)
            {
                System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                {
                    _dialogHostService.MessageBox(MessageType.Message, "信息", ex.Message);
                }));
            }
        }
        private void CalibCalc(AxisPointDisp point, double dist, double angle)
        {
            try
            {
                double[][] diffArray = new double[9][]
                                        { new double[]{ 0,0,0},
                                     new double[]{ dist * 1, 0,0} ,
                                     new double[]{ dist * 1, dist * 1,0},
                                     new double[]{ 0, dist * 1,0},
                                     new double[]{ dist * -1, dist * 1,0},
                                     new double[]{ dist * -1,0,0},
                                     new double[]{ dist * -1, dist * -1,0},
                                     new double[]{ 0, dist * -1,0},
                                     new double[]{ dist * 1, dist * -1,0}};
                //计算9点标定
                double[][] Array1 = new double[9][];
                string filepath = $"Camera\\{(floor == 1 ? "Top" : "Bottom")}\\Calib\\Down";
                string filepath_image = $"Camera\\{(floor == 1 ? "Top" : "Bottom")}\\Calib\\Down\\images";
                HTuple ModelID;
                HOperatorSet.ReadShapeModel(System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath, $"CalibShapeModelID0.shm"), out ModelID);
                CameraAppendHMessage0 = null;
                for (int i = 0; i < 9; i++)
                {
                    try
                    {
                        HObject img;
                        HOperatorSet.ReadImage(out img, System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath_image, $"CalibImage{i + 1}.jpg"));
                        HTuple Row, Column, Angle, Score;
                        HOperatorSet.FindShapeModel(img, ModelID, new HTuple(-180).TupleRad(), new HTuple(360).TupleRad(), 0.5, 1, 0.5, "least_squares", new HTuple(5).TupleConcat(1), 0.75, out Row, out Column, out Angle, out Score);
                        Array1[i] = new double[4] { Row.D, Column.D, point.X + diffArray[i][0], point.Y + diffArray[i][1] };
                        CameraAppendHMessage0 = new HMsgEntry($"{i + 1}", (int)Row.D, (int)Column.D, "green", "image", new HTuple().TupleConcat("box"), new HTuple().TupleConcat("false"));
                    }
                    catch (Exception ex)
                    {
                        Debug.WriteLine(ex.Message);
                        Array1[i] = new double[4] { 0, 0, point.X + diffArray[i][0], point.Y + diffArray[i][1] };
                    }
                }
                HTuple homMat2D;
                HOperatorSet.VectorToHomMat2d(new HTuple(Array1[0][0]).TupleConcat(Array1[1][0]).TupleConcat(Array1[2][0]).TupleConcat(Array1[3][0]).TupleConcat(Array1[4][0]).TupleConcat(Array1[5][0]).TupleConcat(Array1[6][0]).TupleConcat(Array1[7][0]).TupleConcat(Array1[8][0]),
                    new HTuple(Array1[0][1]).TupleConcat(Array1[1][1]).TupleConcat(Array1[2][1]).TupleConcat(Array1[3][1]).TupleConcat(Array1[4][1]).TupleConcat(Array1[5][1]).TupleConcat(Array1[6][1]).TupleConcat(Array1[7][1]).TupleConcat(Array1[8][1]),
                    new HTuple(Array1[0][2]).TupleConcat(Array1[1][2]).TupleConcat(Array1[2][2]).TupleConcat(Array1[3][2]).TupleConcat(Array1[4][2]).TupleConcat(Array1[5][2]).TupleConcat(Array1[6][2]).TupleConcat(Array1[7][2]).TupleConcat(Array1[8][2]),
                    new HTuple(Array1[0][3]).TupleConcat(Array1[1][3]).TupleConcat(Array1[2][3]).TupleConcat(Array1[3][3]).TupleConcat(Array1[4][3]).TupleConcat(Array1[5][3]).TupleConcat(Array1[6][3]).TupleConcat(Array1[7][3]).TupleConcat(Array1[8][3])
                    , out homMat2D);

                double[][] Array2 = new double[3][];
                for (int i = 0; i < 3; i++)
                {
                    try
                    {
                        HObject img;
                        HOperatorSet.ReadImage(out img, System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath_image, $"CalibImage{i + 10}.jpg"));
                        HTuple row, column, angle1, score;
                        HOperatorSet.FindShapeModel(img, ModelID, (new HTuple(-180)).TupleRad(), (new HTuple(360)).TupleRad(), 0.5, 1, 0, "least_squares", 0, 0.9, out row, out column, out angle1, out score);
                        HTuple px, py;
                        HOperatorSet.AffineTransPoint2d(homMat2D, row, column, out px, out py);
                        Array2[i] = new double[2] { px.D, py.D };
                        CameraAppendHMessage0 = new HMsgEntry($"{i + 10}", (int)row.D, (int)column.D, "green", "image", new HTuple().TupleConcat("box"), new HTuple().TupleConcat("false"));
                    }
                    catch (Exception ex)
                    {
                        Array2[i] = new double[2] { 0, 0 };
                    }
                }
                HOperatorSet.ClearShapeModel(ModelID);
                double[] circleCenter = ImageCalc.rotateCenter(Array2[0][0], Array2[0][1], Array2[1][0], Array2[1][1], Array2[2][0], Array2[2][1]);

                double deltax = point.X - circleCenter[0];
                double deltay = point.Y - circleCenter[1];

                HOperatorSet.VectorToHomMat2d(new HTuple(Array1[0][0]).TupleConcat(Array1[1][0]).TupleConcat(Array1[2][0]).TupleConcat(Array1[3][0]).TupleConcat(Array1[4][0]).TupleConcat(Array1[5][0]).TupleConcat(Array1[6][0]).TupleConcat(Array1[7][0]).TupleConcat(Array1[8][0]),
                new HTuple(Array1[0][1]).TupleConcat(Array1[1][1]).TupleConcat(Array1[2][1]).TupleConcat(Array1[3][1]).TupleConcat(Array1[4][1]).TupleConcat(Array1[5][1]).TupleConcat(Array1[6][1]).TupleConcat(Array1[7][1]).TupleConcat(Array1[8][1]),
                new HTuple(Array1[0][2] + deltax).TupleConcat(Array1[1][2] + deltax).TupleConcat(Array1[2][2] + deltax).TupleConcat(Array1[3][2] + deltax).TupleConcat(Array1[4][2] + deltax).TupleConcat(Array1[5][2] + deltax).TupleConcat(Array1[6][2] + deltax).TupleConcat(Array1[7][2] + deltax).TupleConcat(Array1[8][2] + deltax),
                new HTuple(Array1[0][3] + deltay).TupleConcat(Array1[1][3] + deltay).TupleConcat(Array1[2][3] + deltay).TupleConcat(Array1[3][3] + deltay).TupleConcat(Array1[4][3] + deltay).TupleConcat(Array1[5][3] + deltay).TupleConcat(Array1[6][3] + deltay).TupleConcat(Array1[7][3] + deltay).TupleConcat(Array1[8][3] + deltay)
                , out homMat2D);

                HOperatorSet.WriteTuple(homMat2D, System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath, "CalibHomMat2D.tup"));
            }
            catch (Exception ex)
            {
                System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                {
                    _dialogHostService.MessageBox(MessageType.Message, "信息", ex.Message);
                }));
            }
        }
        private void CalibCalcRelation(AxisPointDisp point, int index)
        {
            try
            {
                string filepath = $"Camera\\{(floor == 1 ? "Top" : "Bottom")}\\Calib\\Down";
                string filepath_image = $"Camera\\{(floor == 1 ? "Top" : "Bottom")}\\Calib\\Down\\relation\\{index}";
                HTuple ModelID;
                HOperatorSet.ReadShapeModel(System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath, $"CalibShapeModelID{index - 1}.shm"), out ModelID);
                HTuple homMat2D;
                HOperatorSet.ReadTuple(System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath, "CalibHomMat2D.tup"), out homMat2D);
                CameraAppendHMessage0 = null;
                double[][] Array2 = new double[3][];
                for (int i = 0; i < 3; i++)
                {
                    try
                    {
                        HObject img;
                        HOperatorSet.ReadImage(out img, System.IO.Path.Combine(System.Environment.CurrentDirectory, filepath_image, $"CalibImage{i + 10}.jpg"));
                        HTuple row, column, angle1, score;
                        HOperatorSet.FindShapeModel(img, ModelID, (new HTuple(-180)).TupleRad(), (new HTuple(360)).TupleRad(), 0.5, 1, 0, "least_squares", 0, 0.9, out row, out column, out angle1, out score);
                        HTuple px, py;
                        HOperatorSet.AffineTransPoint2d(homMat2D, row, column, out px, out py);
                        Array2[i] = new double[2] { px.D, py.D };
                        CameraAppendHMessage0 = new HMsgEntry($"{i + 10}", (int)row.D, (int)column.D, "green", "image", new HTuple().TupleConcat("box"), new HTuple().TupleConcat("false"));
                    }
                    catch (Exception ex)
                    {
                        Array2[i] = new double[2] { 0, 0 };
                    }
                }
                HOperatorSet.ClearShapeModel(ModelID);
                double[] circleCenter = ImageCalc.rotateCenter(Array2[0][0], Array2[0][1], Array2[1][0], Array2[1][1], Array2[2][0], Array2[2][1]);
                var _point1 = PointList1.FirstOrDefault(p => p.Name == "BottomCameraCalib_Sucker1");
                double centerx = circleCenter[0] - (point.X - _point1.X);//识别出来的位置，减去移动的距离，等于原来待的位置。如果，没有移动，就是刚好在视野内。
                double centery = circleCenter[1] - (point.Y - _point1.Y);
                using (var sysDb = new SysContext())
                {
                    var _pointDelta = sysDb.XYZRPoints.FirstOrDefault(p => p.Name == $"CenterSuck{index}" && p.Floor == floor);
                    if (_pointDelta != null)
                    {
                        _pointDelta.X = Math.Round(centerx - _point1.X, 3);
                        _pointDelta.Y = Math.Round(centery - _point1.Y, 3);
                        sysDb.SaveChanges();
                    }
                }
            }
            catch (Exception ex)
            {
                System.Windows.Application.Current.Dispatcher.Invoke(new Action(() =>
                {
                    _dialogHostService.MessageBox(MessageType.Message, "信息", ex.Message);
                }));
                
            }
            
        }
        
        #endregion
    }
}
